#include "protocol.h"
#include "crc.h"  /* EASML CRC Library */

/* Initialize protocol context */
void Protocol_Init(Protocol_Context_Type *Context_pst)
{
    /* Initialize receive buffer */
    Protocol_BufferReset(&Context_pst->RxBuffer);
    
    /* Initialize transmit buffer */
    Protocol_BufferReset(&Context_pst->TxBuffer);
    
    /* Initialize parser state */
    Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
    Context_pst->ExpectedBytes = 0u;
    Context_pst->ReceivedBytes = 0u;
    Context_pst->LastRxTime = 0u;
    Context_pst->Status = PROTOCOL_STATUS_OK;
    
    /* Initialize statistics */
    Context_pst->PacketsReceived = 0u;
    Context_pst->PacketsSent = 0u;
    Context_pst->CrcErrors = 0u;
    Context_pst->TimeoutErrors = 0u;
    
    /* Initialize current packet */
    Context_pst->CurrentPacket.Header.SyncByte1 = 0u;
    Context_pst->CurrentPacket.Header.SyncByte2 = 0u;
    Context_pst->CurrentPacket.Header.Command = 0u;
    Context_pst->CurrentPacket.Header.DataLength = 0u;
    Context_pst->CurrentPacket.Crc = 0u;
    Context_pst->CurrentPacket.TotalLength = 0u;
}

/* Build a communication packet */
Protocol_Status_Type Protocol_BuildPacket(Protocol_Packet_Type *Packet_pst,
                                         Protocol_CmdCode_Type Command_e,
                                         const uint8 *Data_cpu8, uint8 DataLength_u8)
{
    uint8 i;
    uint8 temp_buffer[PROTOCOL_MAX_PACKET_SIZE];
    uint8 buffer_index = 0u;
    
    /* Check data length */
    if (DataLength_u8 > PROTOCOL_MAX_DATA_SIZE)
    {
        return PROTOCOL_STATUS_ERROR;
    }
    
    /* Build header */
    Packet_pst->Header.SyncByte1 = PROTOCOL_SYNC_BYTE1;
    Packet_pst->Header.SyncByte2 = PROTOCOL_SYNC_BYTE2;
    Packet_pst->Header.Command = (uint8)Command_e;
    Packet_pst->Header.DataLength = DataLength_u8;
    
    /* Copy data */
    for (i = 0u; i < DataLength_u8; i++)
    {
        Packet_pst->Data[i] = Data_cpu8[i];
    }
    
    /* Prepare buffer for CRC calculation using EASML CRC library */
    temp_buffer[buffer_index++] = Packet_pst->Header.SyncByte1;
    temp_buffer[buffer_index++] = Packet_pst->Header.SyncByte2;
    temp_buffer[buffer_index++] = Packet_pst->Header.Command;
    temp_buffer[buffer_index++] = Packet_pst->Header.DataLength;
    
    for (i = 0u; i < DataLength_u8; i++)
    {
        temp_buffer[buffer_index++] = Packet_pst->Data[i];
    }
    
    /* Calculate CRC using EASML CRC16 function */
    Packet_pst->Crc = Protocol_CalculateCrc(temp_buffer, buffer_index);
    
    /* Set total packet length */
    Packet_pst->TotalLength = PROTOCOL_HEADER_SIZE + DataLength_u8 + 2u;
    
    return PROTOCOL_STATUS_OK;
}

/* Calculate CRC using EASML CRC library */
uint16 Protocol_CalculateCrc(const uint8 *Data_cpu8, uint16 Length_u16)
{
    uint16 crc_result;
    uint16 i;
    
    /* Initialize CRC using EASML CRC16 with polynomial 0x8005 (IBM) */
    Crc_StateCRC16_Type crc_state;
    Crc_ParamCRC16_Type crc_param;
    
    /* Set CRC parameters */
    crc_param.Polynom_u16 = 0x8005u;     /* IBM polynomial */
    crc_param.Seed_u16 = 0x0000u;        /* Initial value */
    crc_param.XorOut_u16 = 0x0000u;      /* Final XOR value */
    crc_param.RefIn_b = TRUE;            /* Reflect input */
    crc_param.RefOut_b = TRUE;           /* Reflect output */
    
    /* Initialize CRC state */
    Crc_CRC16SetState(&crc_state, crc_param.Seed_u16);
    
    /* Process each byte using EASML CRC calculation */
    for (i = 0u; i < Length_u16; i++)
    {
        Crc_CRC16Calc(Data_cpu8[i], &crc_state, &crc_param);
    }
    
    /* Get final CRC result */
    crc_result = Crc_CRC16Out_u16(&crc_state) ^ crc_param.XorOut_u16;
    
    return crc_result;
}

/* Verify CRC of received packet */
boolean Protocol_VerifyCrc(const Protocol_Packet_Type *Packet_cpst)
{
    uint8 temp_buffer[PROTOCOL_MAX_PACKET_SIZE];
    uint8 buffer_index = 0u;
    uint16 calculated_crc;
    uint8 i;
    
    /* Prepare buffer for CRC calculation */
    temp_buffer[buffer_index++] = Packet_cpst->Header.SyncByte1;
    temp_buffer[buffer_index++] = Packet_cpst->Header.SyncByte2;
    temp_buffer[buffer_index++] = Packet_cpst->Header.Command;
    temp_buffer[buffer_index++] = Packet_cpst->Header.DataLength;
    
    for (i = 0u; i < Packet_cpst->Header.DataLength; i++)
    {
        temp_buffer[buffer_index++] = Packet_cpst->Data[i];
    }
    
    /* Calculate CRC */
    calculated_crc = Protocol_CalculateCrc(temp_buffer, buffer_index);
    
    /* Compare with received CRC */
    return (calculated_crc == Packet_cpst->Crc);
}

/* Receive and parse incoming byte */
Protocol_Status_Type Protocol_ReceiveByte(Protocol_Context_Type *Context_pst,
                                         uint8 ReceivedByte_u8, uint32 CurrentTime_u32)
{
    Protocol_Status_Type status = PROTOCOL_STATUS_OK;
    
    /* Check for timeout */
    if ((CurrentTime_u32 - Context_pst->LastRxTime) > PROTOCOL_TIMEOUT_MS)
    {
        Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
        Context_pst->TimeoutErrors++;
    }
    
    Context_pst->LastRxTime = CurrentTime_u32;
    
    /* State machine for packet parsing */
    switch (Context_pst->ParseState)
    {
        case PARSE_STATE_WAIT_SYNC1:
            if (ReceivedByte_u8 == PROTOCOL_SYNC_BYTE1)
            {
                Context_pst->CurrentPacket.Header.SyncByte1 = ReceivedByte_u8;
                Context_pst->ParseState = PARSE_STATE_WAIT_SYNC2;
            }
            break;
            
        case PARSE_STATE_WAIT_SYNC2:
            if (ReceivedByte_u8 == PROTOCOL_SYNC_BYTE2)
            {
                Context_pst->CurrentPacket.Header.SyncByte2 = ReceivedByte_u8;
                Context_pst->ParseState = PARSE_STATE_WAIT_CMD;
            }
            else
            {
                Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
            }
            break;
            
        case PARSE_STATE_WAIT_CMD:
            Context_pst->CurrentPacket.Header.Command = ReceivedByte_u8;
            Context_pst->ParseState = PARSE_STATE_WAIT_LENGTH;
            break;
            
        case PARSE_STATE_WAIT_LENGTH:
            if (ReceivedByte_u8 <= PROTOCOL_MAX_DATA_SIZE)
            {
                Context_pst->CurrentPacket.Header.DataLength = ReceivedByte_u8;
                Context_pst->ExpectedBytes = ReceivedByte_u8;
                Context_pst->ReceivedBytes = 0u;
                
                if (ReceivedByte_u8 > 0u)
                {
                    Context_pst->ParseState = PARSE_STATE_WAIT_DATA;
                }
                else
                {
                    Context_pst->ParseState = PARSE_STATE_WAIT_CRC;
                    Context_pst->ExpectedBytes = 2u;
                    Context_pst->ReceivedBytes = 0u;
                }
            }
            else
            {
                Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
                status = PROTOCOL_STATUS_PARSE_ERROR;
            }
            break;
            
        case PARSE_STATE_WAIT_DATA:
            Context_pst->CurrentPacket.Data[Context_pst->ReceivedBytes] = ReceivedByte_u8;
            Context_pst->ReceivedBytes++;
            
            if (Context_pst->ReceivedBytes >= Context_pst->ExpectedBytes)
            {
                Context_pst->ParseState = PARSE_STATE_WAIT_CRC;
                Context_pst->ExpectedBytes = 2u;
                Context_pst->ReceivedBytes = 0u;
            }
            break;
            
        case PARSE_STATE_WAIT_CRC:
            if (Context_pst->ReceivedBytes == 0u)
            {
                Context_pst->CurrentPacket.Crc = (uint16)ReceivedByte_u8;
            }
            else
            {
                Context_pst->CurrentPacket.Crc |= ((uint16)ReceivedByte_u8) << 8;
            }
            
            Context_pst->ReceivedBytes++;
            
            if (Context_pst->ReceivedBytes >= 2u)
            {
                Context_pst->ParseState = PARSE_STATE_PROCESS;
            }
            break;
            
        case PARSE_STATE_PROCESS:
            /* This shouldn't happen - packet should be processed elsewhere */
            Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
            break;
            
        default:
            Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
            break;
    }
    
    /* Check if packet is ready for processing */
    if (Context_pst->ParseState == PARSE_STATE_PROCESS)
    {
        Context_pst->CurrentPacket.TotalLength = PROTOCOL_HEADER_SIZE + 
                                               Context_pst->CurrentPacket.Header.DataLength + 2u;
        
        /* Verify CRC using EASML */
        if (Protocol_VerifyCrc(&Context_pst->CurrentPacket))
        {
            Context_pst->PacketsReceived++;
            status = PROTOCOL_STATUS_OK;
        }
        else
        {
            Context_pst->CrcErrors++;
            status = PROTOCOL_STATUS_CRC_ERROR;
        }
        
        /* Reset parser for next packet */
        Context_pst->ParseState = PARSE_STATE_WAIT_SYNC1;
    }
    
    return status;
}

/* Build response packet */
Protocol_Status_Type Protocol_BuildResponse(Protocol_Packet_Type *Response_pst,
                                           Protocol_CmdCode_Type ResponseCode_e,
                                           const void *ResponseData_cpv, uint8 DataLength_u8)
{
    const uint8 *data_pu8 = (const uint8 *)ResponseData_cpv;
    
    return Protocol_BuildPacket(Response_pst, ResponseCode_e, data_pu8, DataLength_u8);
}

/* Build error response */
Protocol_Status_Type Protocol_BuildErrorResponse(Protocol_Packet_Type *Response_pst,
                                                Protocol_Status_Type ErrorCode_e)
{
    uint8 error_data = (uint8)ErrorCode_e;
    
    return Protocol_BuildPacket(Response_pst, CMD_RESPONSE_ERROR, &error_data, 1u);
}

/* Reset communication buffer */
void Protocol_BufferReset(Protocol_CommBuffer_Type *Buffer_pst)
{
    Buffer_pst->WriteIndex = 0u;
    Buffer_pst->ReadIndex = 0u;
    Buffer_pst->DataAvailable = 0u;
    Buffer_pst->PacketReady = FALSE;
}

/* Write data to buffer */
Protocol_Status_Type Protocol_BufferWrite(Protocol_CommBuffer_Type *Buffer_pst,
                                         const uint8 *Data_cpu8, uint16 Length_u16)
{
    uint16 i;
    uint16 space_available;
    
    space_available = (PROTOCOL_MAX_PACKET_SIZE * 2) - Buffer_pst->DataAvailable;
    
    if (Length_u16 > space_available)
    {
        return PROTOCOL_STATUS_BUFFER_FULL;
    }
    
    for (i = 0u; i < Length_u16; i++)
    {
        Buffer_pst->Buffer[Buffer_pst->WriteIndex] = Data_cpu8[i];
        Buffer_pst->WriteIndex++;
        
        if (Buffer_pst->WriteIndex >= (PROTOCOL_MAX_PACKET_SIZE * 2))
        {
            Buffer_pst->WriteIndex = 0u;
        }
    }
    
    Buffer_pst->DataAvailable += Length_u16;
    
    return PROTOCOL_STATUS_OK;
}

/* Read data from buffer */
Protocol_Status_Type Protocol_BufferRead(Protocol_CommBuffer_Type *Buffer_pst,
                                        uint8 *Data_pu8, uint16 Length_u16)
{
    uint16 i;
    
    if (Length_u16 > Buffer_pst->DataAvailable)
    {
        return PROTOCOL_STATUS_ERROR;
    }
    
    for (i = 0u; i < Length_u16; i++)
    {
        Data_pu8[i] = Buffer_pst->Buffer[Buffer_pst->ReadIndex];
        Buffer_pst->ReadIndex++;
        
        if (Buffer_pst->ReadIndex >= (PROTOCOL_MAX_PACKET_SIZE * 2))
        {
            Buffer_pst->ReadIndex = 0u;
        }
    }
    
    Buffer_pst->DataAvailable -= Length_u16;
    
    return PROTOCOL_STATUS_OK;
}

/* Get available data in buffer */
uint16 Protocol_BufferAvailable(const Protocol_CommBuffer_Type *Buffer_cpst)
{
    return Buffer_cpst->DataAvailable;
}

/* Send packet */
Protocol_Status_Type Protocol_SendPacket(Protocol_Context_Type *Context_pst,
                                        const Protocol_Packet_Type *Packet_cpst)
{
    uint8 tx_buffer[PROTOCOL_MAX_PACKET_SIZE];
    uint8 buffer_index = 0u;
    uint8 i;
    Protocol_Status_Type status;
    
    /* Serialize packet to buffer */
    tx_buffer[buffer_index++] = Packet_cpst->Header.SyncByte1;
    tx_buffer[buffer_index++] = Packet_cpst->Header.SyncByte2;
    tx_buffer[buffer_index++] = Packet_cpst->Header.Command;
    tx_buffer[buffer_index++] = Packet_cpst->Header.DataLength;
    
    for (i = 0u; i < Packet_cpst->Header.DataLength; i++)
    {
        tx_buffer[buffer_index++] = Packet_cpst->Data[i];
    }
    
    /* Add CRC */
    tx_buffer[buffer_index++] = (uint8)(Packet_cpst->Crc & 0xFFu);
    tx_buffer[buffer_index++] = (uint8)((Packet_cpst->Crc >> 8) & 0xFFu);
    
    /* Write to transmit buffer */
    status = Protocol_BufferWrite(&Context_pst->TxBuffer, tx_buffer, buffer_index);
    
    if (status == PROTOCOL_STATUS_OK)
    {
        Context_pst->PacketsSent++;
    }
    
    return status;
}

/* Data conversion functions */
sint16 Protocol_FloatToFixed(float32 Value_f32, uint8 FracBits_u8)
{
    float32 scale = 1.0f;
    uint8 i;
    
    /* Calculate scaling factor */
    for (i = 0u; i < FracBits_u8; i++)
    {
        scale *= 2.0f;
    }
    
    return (sint16)(Value_f32 * scale);
}

float32 Protocol_FixedToFloat(sint16 Value_s16, uint8 FracBits_u8)
{
    float32 scale = 1.0f;
    uint8 i;
    
    /* Calculate scaling factor */
    for (i = 0u; i < FracBits_u8; i++)
    {
        scale *= 2.0f;
    }
    
    return (float32)Value_s16 / scale;
}

/* Utility functions for data packing/unpacking */
void Protocol_PackU16(uint8 *Buffer_pu8, uint16 Value_u16)
{
    Buffer_pu8[0] = (uint8)(Value_u16 & 0xFFu);
    Buffer_pu8[1] = (uint8)((Value_u16 >> 8) & 0xFFu);
}

uint16 Protocol_UnpackU16(const uint8 *Buffer_cpu8)
{
    return ((uint16)Buffer_cpu8[0]) | (((uint16)Buffer_cpu8[1]) << 8);
}

void Protocol_PackS16(uint8 *Buffer_pu8, sint16 Value_s16)
{
    Protocol_PackU16(Buffer_pu8, (uint16)Value_s16);
}

sint16 Protocol_UnpackS16(const uint8 *Buffer_cpu8)
{
    return (sint16)Protocol_UnpackU16(Buffer_cpu8);
}

void Protocol_PackU32(uint8 *Buffer_pu8, uint32 Value_u32)
{
    Buffer_pu8[0] = (uint8)(Value_u32 & 0xFFu);
    Buffer_pu8[1] = (uint8)((Value_u32 >> 8) & 0xFFu);
    Buffer_pu8[2] = (uint8)((Value_u32 >> 16) & 0xFFu);
    Buffer_pu8[3] = (uint8)((Value_u32 >> 24) & 0xFFu);
}

uint32 Protocol_UnpackU32(const uint8 *Buffer_cpu8)
{
    return ((uint32)Buffer_cpu8[0]) | 
           (((uint32)Buffer_cpu8[1]) << 8) |
           (((uint32)Buffer_cpu8[2]) << 16) |
           (((uint32)Buffer_cpu8[3]) << 24);
}

/* Get communication statistics */
void Protocol_GetStats(const Protocol_Context_Type *Context_cpst,
                      uint32 *PacketsRx_pu32, uint32 *PacketsTx_pu32,
                      uint32 *CrcErrors_pu32, uint32 *TimeoutErrors_pu32)
{
    *PacketsRx_pu32 = Context_cpst->PacketsReceived;
    *PacketsTx_pu32 = Context_cpst->PacketsSent;
    *CrcErrors_pu32 = Context_cpst->CrcErrors;
    *TimeoutErrors_pu32 = Context_cpst->TimeoutErrors;
}

/* Reset communication statistics */
void Protocol_ResetStats(Protocol_Context_Type *Context_pst)
{
    Context_pst->PacketsReceived = 0u;
    Context_pst->PacketsSent = 0u;
    Context_pst->CrcErrors = 0u;
    Context_pst->TimeoutErrors = 0u;
}
